/**
  ******************************************************************************
  * File Name          : app_mems.c
  * Description        : This file provides code for the configuration
  *                      of the STMicroelectronics.X-CUBE-MEMS1.10.0.0 instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2023 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */

#ifdef __cplusplus
extern "C" {
#endif



/* Includes ------------------------------------------------------------------*/
#include "app_mems.h"
#include "main.h"
#include "usart.h"
#include <stdio.h>
#include "iks4a1_mems_control.h"
#include "iks4a1_conf.h"
#include "motion_sensor.h"
//#include "imu.h"
#include "MahonyAHRS.h"
#include <Altitude.C>


#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define ALGO_FREQ  50U /* Algorithm frequency >= 50Hz */
#define ACC_ODR  ((float)ALGO_FREQ)
#define ACC_FS  4 /* FS = <-4g, 4g> */
#define GYR_ODR  ((float)ALGO_FREQ)
#define GYR_FS  200 /* FS = <+_250> */
/* MOVE_THR_G recommended between 0.15 - 0.30 g, higher value will relax condition on data selection for calibration but
   reduce the accuracy which will be around (moveThresh_g / 10) */

/* Public variables ----------------------------------------------------------*/
//MAC_calibration_mode_t CalibrationMode = DYNAMIC_CALIBRATION;

/* Extern variables ----------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
static MOTION_SENSOR_Axes_t AccValue;
static MOTION_SENSOR_Axes_t GyrValue;
static MOTION_SENSOR_Axes_t MagValue;

static Flote_date FAccValue;
static Flote_date FGyrValue;
static Flote_date FMagValue;
static float PressValue;
static float TempValue;

//static int16_t QvarValue;
//static float HumValue;

/* Private function prototypes -----------------------------------------------*/
static void Init_Sensors(void);

//static void TIM_Config(uint32_t Freq);
void MX_MEMS_Init(void)
{
  /* USER CODE BEGIN SV */

  /* USER CODE END SV */

  /* USER CODE BEGIN MEMS_Init_PreTreatment */

  /* USER CODE END MEMS_Init_PreTreatment */

  /* Initialize the peripherals and the MEMS components */


  /* USER CODE BEGIN MEMS_Init_PostTreatment */
	Init_Sensors();

  /* USER CODE END MEMS_Init_PostTreatment */
}

/*
 * LM background task
 */
void MX_MEMS_Process(void)
{
  /* USER CODE BEGIN MEMS_Process_PreTreatment */
  /* USER CODE END MEMS_Process_PreTreatment */

	//BSP_SENSOR_LSM6DSO16IS_ACC_Enable();
	//BSP_SENSOR_LSM6DSO16IS_GYR_Enable();


	//BSP_SENSOR_LSM6DSO16IS_ACC_GetAxes(&AccValue);
	//BSP_SENSOR_LSM6DSO16IS_GYR_GetAxes(&GyrValue);
	//BSP_SENSOR_MAG_GetAxes(&MagValue);
	//BSP_SENSOR_PRESS_GetValue(&PressValue);

	//imu(AccValue,GyrValue,MagValue);

	//IMU_date(AccValue,GyrValue);
	//IMU_AHRS_G_A(AccValue,GyrValue);
	//IMU_AHRS_G_A_M(AccValue,GyrValue,MagValue);


    //printf("{ACC:%d,%d,%d}\n",(int)AccValue.x,(int)AccValue.y,(int)AccValue.z);
    //printf("{GYR:%d,%d,%d}\n",(int)GyrValue.x,(int)GyrValue.y,(int)GyrValue.z);
    //printf("{MAG:%d,%d,%d}\n",(int)MagValue.x,(int)MagValue.y,(int)MagValue.z);
    //printf("{PRE:%d}\n",(int)PressValue);
	//HAL_Delay(10);

  /* USER CODE BEGIN MEMS_Process_PostTreatment */

  /* USER CODE END MEMS_Process_PostTreatment */
}

/**
  * @brief  Initialize all sensors
  * @param  None
  * @retval None
  */
static void Init_Sensors(void)
{
	BSP_SENSOR_LSM6DSV16X_ACC_Init();
	BSP_SENSOR_LSM6DSO16IS_ACC_Init();
	BSP_SENSOR_LIS2DUXS12_ACC_Init();

	BSP_SENSOR_LSM6DSO16IS_GYR_Init();
	BSP_SENSOR_LSM6DSV16X_GYR_Init();

	BSP_SENSOR_MAG_Init();

	BSP_SENSOR_PRESS_Init();
	BSP_SENSOR_TEMP_Init();
	BSP_SENSOR_HUM_Init();

	BSP_SENSOR_LSM6DSO16IS_ACC_SetOutputDataRate(ACC_ODR);
	BSP_SENSOR_LSM6DSO16IS_ACC_SetFullScale(ACC_FS);
	//BSP_SENSOR_GYR_SetOutputDataRate(GYR_ODR);
	//BSP_SENSOR_GYR_SetFullScale(GYR_FS);
	BSP_SENSOR_LSM6DSV16X_ACC_SetOutputDataRate(ACC_ODR);
	BSP_SENSOR_LSM6DSV16X_ACC_SetFullScale(ACC_FS);
	BSP_SENSOR_LIS2DUXS12_ACC_SetOutputDataRate(ACC_ODR);
	BSP_SENSOR_LIS2DUXS12_ACC_SetFullScale(ACC_FS);
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *hitm)
{

	//IMU_date(AccValue,GyrValue);
	//IMU_AHRS_G_A(AccValue,GyrValue);
	//IMU_AHRS_G_A_M(AccValue,GyrValue,MagValue);

	BSP_SENSOR_LSM6DSO16IS_ACC_GetAxes(&AccValue);
	BSP_SENSOR_LSM6DSO16IS_GYR_GetAxes(&GyrValue);
	//BSP_SENSOR_LSM6DSV16X_ACC_GetAxes(&AccValue);
	//BSP_SENSOR_LSM6DSV16X_GYR_GetAxes(&GyrValue);
	BSP_SENSOR_MAG_GetAxes(&MagValue);
	BSP_SENSOR_PRESS_GetValue(&PressValue);
	BSP_SENSOR_TEMP_GetValue(&TempValue);
	//printf("{ACC:%d,%d,%d}\n",(int)AccValue.x,(int)AccValue.y,(int)AccValue.z);
	//printf("{GYR:%d,%d,%d}\n",(int)GyrValue.x,(int)GyrValue.y,(int)GyrValue.z);
	//printf("{MAG:%d,%d,%d}\n",(int)MagValue.x,(int)MagValue.y,(int)MagValue.z);
	//printf("{PRE:%d}\n",(int)PressValue);

	imu(AccValue,GyrValue,MagValue);

	//Magnetometer(FMagValue.x,FMagValue.x,FMagValue.x);

	MahonyAHRSupdateIMU_G_A(FAccValue,FGyrValue);
	//MahonyAHRSupdate_G_A_M(FAccValue,FGyrValue,FMagValue);
	//HAL_Delay(100);


	Altitude(PressValue,TempValue);

	//HAL_GPIO_TogglePin(led_GPIO_Port,led_Pin);
}

void imu(MOTION_SENSOR_Axes_t AccValue,
		 MOTION_SENSOR_Axes_t GyrValue,
		 MOTION_SENSOR_Axes_t MagValue)
{
	FAccValue.x=(AccValue.x)*0.0048828;
	FAccValue.y=(AccValue.y)*0.0048828;
	FAccValue.z=(AccValue.z)*0.0048828;

	FGyrValue.x=((GyrValue.x)*3.14/16.384)/180/1000;
	FGyrValue.y=((GyrValue.y)*3.14/16.384)/180/1000;
	FGyrValue.z=((GyrValue.z)*3.14/16.384)/180/1000;

	FMagValue.x=MagValue.x*1.5;
	FMagValue.y=MagValue.y*1.5;
	FMagValue.z=MagValue.z*1.5;
	//return FAccValue,FGyrValue,FMagValue;
}


PUTCHAR_PROTOTYPE
{
	HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
	return ch;
}


#ifdef __cplusplus
}
#endif
